#pragma once
#include<rclcpp/rclcpp.hpp>
#include<memory>
#include<fstream>
#include<iostream>
#include<math.h>
#include<sys/types.h>
#include<signal.h>
#include<poll.h>
#include<message_filters/subscriber.h>
#include<message_filters/time_synchronizer.h>
#include<message_filters/sync_policies/approximate_time.h>
#include<eigen3/Eigen/Eigen>
#include"base_interface/msg/control_cmd.hpp"
#include"base_interface/msg/hit.hpp"
#include"base_interface/msg/mcu_comm.hpp"
#include"mcu_pack/user_inc.h"
// #include"base_interface/usr_text_output.hpp"

using namespace std;
using namespace std::chrono_literals;// 可以使用带单位的时间量 比如下面用的1s 500ms
using namespace Eigen;
#define PI 3.1415926

//上位机->下位机数据长度
#define TX_MCU_LENGTH (16 * 4)
//上位机->下位机 包含帧头帧尾数据总长度
#define TX_DATA_LEN (TX_MCU_LENGTH + 4)

//上位机<-下位机数据长度
#define RX_MCU_LENGTH (15 * 4)
//上位机<-下位机 包含帧头帧尾数据总长度
#define RX_DATA_LEN (RX_MCU_LENGTH + 4)





typedef union 
{
  float daraF[TX_MCU_LENGTH / 4];
  
  char  dataC[TX_MCU_LENGTH];
}Tx_MCU;

typedef union 
{
  float dataF[RX_MCU_LENGTH / 4];

  char  dataC[RX_MCU_LENGTH];
}Rx_MCU;


class commSerial
{
private:
  /*data*/
  struct termios termAttr;
  struct sigaction saio;
  int ifSerial = 0;
  const char *this_port;
  uint32_t this_baudrate;
  int handle;

public:
  commSerial(const char *port = "", uint32_t baudrate = B115200);
  ~commSerial();
  void commSerialInit();
  int commSerialRead();
  // void commSerialWrite(char *data);
  void commSerialWrite(void);
  int commSerialIsopen();
  unsigned char rxbuf[RX_DATA_LEN];
  unsigned char txbuf[TX_DATA_LEN];

};


class MyTestMcuNode : public rclcpp::Node
{
public:
  MyTestMcuNode();
  ~MyTestMcuNode(){};


  void TimeCallback();//声明

  void RxCallback();


  void ControlCallback(const base_interface::msg::ControlCmd::SharedPtr cmd_vel_);

  void ToSlaveCommunicate();

  void HitControlCallback(const base_interface::msg::Hit::SharedPtr hit_msg);


private:
  // float hit_vel_msg;
  // float hit_flag_msg;
  float CubeMar_Moter_one;
  float CubeMar_Moter_two;
  float CubeMar_Moter_thr;
  float CubeMar_Moter_fou;


  float hit_target_pitch_motor;
  float hit_target_roll_motor;
  float hit_target_yaw_motor;
  float hit_vel_motor;
  int hit_mcu_flag;//上位机给下位机发的击球flag
  rclcpp::Publisher<base_interface::msg::McuComm>::SharedPtr mcu_comm_pub;
  rclcpp::Subscription<base_interface::msg::ControlCmd>::SharedPtr control_cmd_sub;
  rclcpp::Subscription<base_interface::msg::Hit>::SharedPtr hit_cmd_sub;

  rclcpp::TimerBase::SharedPtr timer_;//定时器


};

//轮速到车速，底盘分解逆运算
Vector2d Wheels_to_vehicle_speed(float v1, float v2, float v3, float v4);